#include "LocalMapping.h"
#include <unistd.h>
#include <opencv2/core/core.hpp>


#include "g2o/g2o/core/block_solver.h"
#include "g2o/g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/g2o/solvers/linear_solver_eigen.h"
#include "g2o/g2o/types/types_six_dof_expmap.h"
#include "g2o/g2o/core/robust_kernel_impl.h"
#include "g2o/g2o/solvers/linear_solver_dense.h"
#include "g2o/g2o/types/types_seven_dof_expmap.h"

#include "Converter.h"
#include <stdint.h>


#define TH_LOW  250


using namespace std;
using namespace cv;

LocalMapping::LocalMapping(Map* pMap):
	m_Map(pMap),
	m_ResetRequested(false),
	m_FinishRequested(false),
	m_Finished(true),
    m_AbortBA(false),
    m_Stopped(false),
    m_StopRequested(false),
    m_NotStop(false),
    m_AcceptKeyFrames(true)
{


}

LocalMapping::~LocalMapping()
{

}


void LocalMapping::Run()
{
	m_Finished = false;

	while(1)
	{
		// Tracking will see that Local Mapping is busy
		SetAcceptKeyFrames(false);

		// Check if there are keyframes in the queue
		if(CheckNewKeyFrames())
		{
			//insertion in Map
			ProcessNewKeyFrame();

			// Check recent MapPoints
			MapPointCulling();

			// Triangulate new MapPoints
			CreateNewMapPoints();

			if(!CheckNewKeyFrames())
			{
				// Find more matches in neighbor keyframes and fuse point duplications
				SearchInNeighbors();
			}

			m_AbortBA = false;
			if(!CheckNewKeyFrames() && !stopRequested())
			{
				// Local BA
				if(m_Map->KeyFramesSize() > 2)
				{
					LocalBundleAdjustment(m_CurrentKeyFrame,&m_AbortBA, m_Map);
				}

				// Check redundant local Keyframes
				KeyFrameCulling();
			}

			//m_LoopCloser->InsertKeyFrame(m_CurrentKeyFrame);
		}
		else if(Stop())
		{
			// Safe area to stop
			while(isStopped() && !CheckFinish())
			{
				usleep(3000);
			}

			if(CheckFinish())
			{
				break;
			}
		}

		ResetIfRequested();

		// Tracking will see that Local Mapping is busy
		SetAcceptKeyFrames(true);

		if(CheckFinish())
		{
			break;
		}

		usleep(3000);
	}
}


void LocalMapping::InsertKeyFrame(KeyFrame* pKF)
{
	unique_lock<mutex> lock(m_MutexNewKFs);
	m_listNewKeyFrames.push_back(pKF);
	m_AbortBA=true;
}


void LocalMapping::RequestStop()
{
	{
		unique_lock<mutex> lock(m_MutexStop);
		m_StopRequested = true;
	}

	{
		unique_lock<mutex> lock2(m_MutexNewKFs);
		m_AbortBA = true;
	}
}


void LocalMapping::RequestReset()
{
	{
		unique_lock<mutex> lock(m_MutexReset);
		 m_ResetRequested = true;
	}

	while(1)
	{
		{
			unique_lock<mutex> lock2(m_MutexReset);
			if(!m_ResetRequested)
			{
				break;
			}
		}

		usleep(3000);
	}
}


bool LocalMapping::Stop()
{
	unique_lock<mutex> lock(m_MutexStop);
	if(m_StopRequested && !m_NotStop)
	{
		m_Stopped = true;
		return true;
	}

	return false;
}


void LocalMapping::Release()
{
	unique_lock<mutex> lock(m_MutexStop);
	unique_lock<mutex> lock2(m_MutexFinish);

	if(m_Finished)
	{
		return;
	}

	m_Stopped = false;
	m_StopRequested = false;
	for(list<KeyFrame*>::iterator it = m_listNewKeyFrames.begin(), end=m_listNewKeyFrames.end(); it!=end; it++)
	{
		delete *it;
	}

	m_listNewKeyFrames.clear();
}


bool LocalMapping::isStopped()
{
	unique_lock<mutex> lock(m_MutexStop);
	return m_Stopped;
}


bool LocalMapping::stopRequested()
{
	unique_lock<mutex> lock(m_MutexStop);
	return m_StopRequested;
}


bool LocalMapping::AcceptKeyFrames()
{
	unique_lock<mutex> lock(m_MutexAccept);
	return m_AcceptKeyFrames;
}


void LocalMapping::SetAcceptKeyFrames(bool flag)
{
	unique_lock<mutex> lock(m_MutexAccept);
	m_AcceptKeyFrames = flag;
}


bool LocalMapping::SetNotStop(bool flag)
{
	unique_lock<mutex> lock(m_MutexStop);
	if(flag && m_Stopped)
	{
		return false;
	}

	m_NotStop = flag;
	return true;
}


void LocalMapping::InterruptBA()
{
	m_AbortBA = true;
}


void LocalMapping::RequestFinish()
{
	unique_lock<mutex> lock(m_MutexFinish);
	m_FinishRequested = true;
}


bool LocalMapping::isFinished()
{
	unique_lock<mutex> lock(m_MutexFinish);
	return m_Finished;
}


int LocalMapping::KeyframesSize()
{
	unique_lock<std::mutex> lock(m_MutexNewKFs);
	return m_listNewKeyFrames.size();
}


bool LocalMapping::CheckNewKeyFrames()
{
	unique_lock<mutex> lock(m_MutexNewKFs);
	return(!m_listNewKeyFrames.empty());
}


void LocalMapping::ProcessNewKeyFrame()
{
	{
		unique_lock<mutex> lock(m_MutexNewKFs);
		m_CurrentKeyFrame = m_listNewKeyFrames.front();
		m_listNewKeyFrames.pop_front();
	}

	// Associate MapPoints to the new keyframe and update normal and descriptor
	const vector<MapPoint*> vpMapPointMatches = m_CurrentKeyFrame->GetMapPointMatches();
	for(size_t i=0; i<vpMapPointMatches.size(); i++)
	{
		 MapPoint* pMP = vpMapPointMatches[i];
		 if(pMP)
		 {
			if(!pMP->isBadMapPoint())
			{
				if(!pMP->IsInKeyFrame(m_CurrentKeyFrame))
				{
					pMP->AddObservation(m_CurrentKeyFrame, i);
					pMP->UpdateNormalAndDepth();
					pMP->ComputeDistinctiveDescriptors();
				}
				else
				{
					// this can only happen for new stereo points inserted by the Tracking
					m_listRecentAddedMapPoints.push_back(pMP);
				}
			}
		 }
	}

	// Update links in the Covisibility Graph
	m_CurrentKeyFrame->UpdateConnections();

	// Insert Keyframe in Map
	m_Map->AddKeyFrame(m_CurrentKeyFrame);
}


void LocalMapping::CreateNewMapPoints()
{
	// Retrieve neighbor keyframes in covisibility graph
	int nn = 10;
	vector<KeyFrame*> vpNeighKFs = m_CurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);

	cv::Mat Rcw1 = m_CurrentKeyFrame->GetRotation();
	cv::Mat Rwc1 = Rcw1.t();
	cv::Mat tcw1 = m_CurrentKeyFrame->GetTranslation();
	cv::Mat Tcw1(3,4,CV_32F);
	Rcw1.copyTo(Tcw1.colRange(0,3));
	tcw1.copyTo(Tcw1.col(3));
	cv::Mat Ow1 = m_CurrentKeyFrame->GetCameraCenter();

	float fx1 = m_CurrentKeyFrame->m_fx;
	float fy1 = m_CurrentKeyFrame->m_fy;
	float cx1 = m_CurrentKeyFrame->m_cx;
	float cy1 = m_CurrentKeyFrame->m_cy;
	float invfx1 = m_CurrentKeyFrame->m_invfx;
	float invfy1 = m_CurrentKeyFrame->m_invfy;

	int nnew=0;
	// Search matches with epipolar restriction and triangulate
	for(size_t i=0; i<vpNeighKFs.size(); i++)
	{
		if(i>0 && CheckNewKeyFrames())
		{
			return;
		}

		KeyFrame* pKF2 = vpNeighKFs[i];
		// Check first that baseline is not too short
		cv::Mat Ow2 = pKF2->GetCameraCenter();
		cv::Mat vBaseline = Ow2-Ow1;
		float baseline = cv::norm(vBaseline);

		if(baseline < pKF2->m_baseline)
		{
			continue;
		}

		// Compute Fundamental Matrix
        cv::Mat F12 = ComputeF12(m_CurrentKeyFrame, pKF2);

		// Search matches that fullfil epipolar constraint
		vector<pair<size_t,size_t> > vMatchedIndices;
		SearchForTriangulation(m_CurrentKeyFrame, pKF2, F12, vMatchedIndices);

		cv::Mat Rcw2 = pKF2->GetRotation();
		cv::Mat Rwc2 = Rcw2.t();
		cv::Mat tcw2 = pKF2->GetTranslation();
		cv::Mat Tcw2(3,4,CV_32F);
		Rcw2.copyTo(Tcw2.colRange(0,3));
		tcw2.copyTo(Tcw2.col(3));

		float fx2 = pKF2->m_fx;
		float fy2 = pKF2->m_fy;
		float cx2 = pKF2->m_cx;
		float cy2 = pKF2->m_cy;
		float invfx2 = pKF2->m_invfx;
		float invfy2 = pKF2->m_invfy;

		// Triangulate each match
		const int nmatches = vMatchedIndices.size();
		for(int ikp=0; ikp<nmatches; ikp++)
		{
			int idx1 = vMatchedIndices[ikp].first;
			int idx2 = vMatchedIndices[ikp].second;

			const cv::KeyPoint &kp1 = m_CurrentKeyFrame->m_vectorKeyPointsUn[idx1];
			float kp1_ur = m_CurrentKeyFrame->m_vectorURight[idx1];
			bool bStereo1 = kp1_ur>=0;

			const cv::KeyPoint &kp2 = pKF2->m_vectorKeyPointsUn[idx2];
			float kp2_ur = pKF2->m_vectorURight[idx2];
			bool bStereo2 = kp2_ur>=0;

			// Check parallax between rays
			cv::Mat xn1 = (cv::Mat_<float>(3,1) << (kp1.pt.x-cx1)*invfx1, (kp1.pt.y-cy1)*invfy1, 1.0);
			cv::Mat xn2 = (cv::Mat_<float>(3,1) << (kp2.pt.x-cx2)*invfx2, (kp2.pt.y-cy2)*invfy2, 1.0);

			cv::Mat ray1 = Rwc1 * xn1;
			cv::Mat ray2 = Rwc2 * xn2;

			float cosParallaxRays = ray1.dot(ray2) / (cv::norm(ray1)*cv::norm(ray2));

			float cosParallaxStereo = cosParallaxRays+1;
			float cosParallaxStereo1 = cosParallaxStereo;
			float cosParallaxStereo2 = cosParallaxStereo;

			if(bStereo1)
			{
				cosParallaxStereo1 = cos(2 * atan2(m_CurrentKeyFrame->m_baseline / 2, m_CurrentKeyFrame->m_vectorDepth[idx1]));
			}
			else if(bStereo2)
			{
				cosParallaxStereo2 = cos(2 * atan2(pKF2->m_baseline / 2, pKF2->m_vectorDepth[idx2]));
			}

			cosParallaxStereo = min(cosParallaxStereo1, cosParallaxStereo2);

			cv::Mat x3D;
			if(cosParallaxRays<cosParallaxStereo && cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998))
			{
				// Linear Triangulation Method
				cv::Mat A(4,4,CV_32F);
				A.row(0) = xn1.at<float>(0)*Tcw1.row(2)-Tcw1.row(0);
				A.row(1) = xn1.at<float>(1)*Tcw1.row(2)-Tcw1.row(1);
				A.row(2) = xn2.at<float>(0)*Tcw2.row(2)-Tcw2.row(0);
				A.row(3) = xn2.at<float>(1)*Tcw2.row(2)-Tcw2.row(1);

				cv::Mat w,u,vt;
				cv::SVD::compute(A, w, u, vt, cv::SVD::MODIFY_A| cv::SVD::FULL_UV);

				x3D = vt.row(3).t();
				if(x3D.at<float>(3) == 0)
				{
					continue;
				}

				// Euclidean coordinates
				x3D = x3D.rowRange(0,3) / x3D.at<float>(3);
			}
			else if(bStereo1 && cosParallaxStereo1<cosParallaxStereo2)
			{
				x3D = m_CurrentKeyFrame->UnprojectStereo(idx1);
			}
			else if(bStereo2 && cosParallaxStereo2<cosParallaxStereo1)
			{
				x3D = pKF2->UnprojectStereo(idx2);
			}
			else
			{
				continue;
			}

			cv::Mat x3Dt = x3D.t();
			//Check triangulation in front of cameras
			float z1 = Rcw1.row(2).dot(x3Dt)+tcw1.at<float>(2);
			if(z1 <= 0)
			{
				continue;
			}

			float z2 = Rcw2.row(2).dot(x3Dt)+tcw2.at<float>(2);
			if(z2 <= 0)
			{
				continue;
			}

			//Check reprojection error in first keyframe
			float sigmaSquare1 = 1.0f / 0.69f;
			float x1 = Rcw1.row(0).dot(x3Dt)+tcw1.at<float>(0);
			float y1 = Rcw1.row(1).dot(x3Dt)+tcw1.at<float>(1);
			float invz1 = 1.0 / z1;

			if(!bStereo1)
			{
				float u1 = fx1 * x1 * invz1 + cx1;
				float v1 = fy1 * y1 * invz1 + cy1;
				float errX1 = u1 - kp1.pt.x;
				float errY1 = v1 - kp1.pt.y;

				 if((errX1 * errX1 + errY1 * errY1) > 5.991 * sigmaSquare1)
				 {
					continue;
				 }
			}
			else
			{
				float u1 = fx1 * x1 * invz1 + cx1;
				float u1_r = u1 - m_CurrentKeyFrame->m_baselineFx * invz1;
				float v1 = fy1 * y1 * invz1 + cy1;
				float errX1 = u1 - kp1.pt.x;
				float errY1 = v1 - kp1.pt.y;
				float errX1_r = u1_r - kp1_ur;
				if((errX1 * errX1 + errY1 * errY1 + errX1_r * errX1_r) > 7.815 * sigmaSquare1)
				{
					continue;
				}
			}

			//Check reprojection error in second keyframe
			float sigmaSquare2 = 1.0f / 0.69f;
			float x2 = Rcw2.row(0).dot(x3Dt)+tcw2.at<float>(0);
			float y2 = Rcw2.row(1).dot(x3Dt)+tcw2.at<float>(1);
			float invz2 = 1.0 / z2;

			if(!bStereo2)
			{
				float u2 = fx2 * x2 * invz2 + cx2;
				float v2 = fy2 * y2 * invz2 + cy2;
				float errX2 = u2 - kp2.pt.x;
				float errY2 = v2 - kp2.pt.y;
				if((errX2 * errX2 + errY2 * errY2) > 5.991 * sigmaSquare2)
				{
					continue;
				}
			}
			else
			{
				float u2 = fx2 * x2 * invz2 + cx2;
				float u2_r = u2 - m_CurrentKeyFrame->m_baselineFx * invz2;
				float v2 = fy2 * y2 * invz2 + cy2;
				float errX2 = u2 - kp2.pt.x;
				float errY2 = v2 - kp2.pt.y;
				float errX2_r = u2_r - kp2_ur;
				if((errX2 * errX2 + errY2 * errY2 + errX2_r * errX2_r) > 7.815 * sigmaSquare2)
				{
					continue;
				}
			}

			//Check scale consistency
			cv::Mat normal1 = x3D-Ow1;
			float dist1 = cv::norm(normal1);

			cv::Mat normal2 = x3D-Ow2;
			float dist2 = cv::norm(normal2);

			if(dist1==0 || dist2==0)
			{
				continue;
			}

			// Triangulation is succesfull
			MapPoint* pMP = new MapPoint(x3D,m_CurrentKeyFrame ,m_Map);

			pMP->AddObservation(m_CurrentKeyFrame, idx1);
			pMP->AddObservation(pKF2, idx2);

			m_CurrentKeyFrame->AddMapPoint(pMP, idx1);
			pKF2->AddMapPoint(pMP, idx2);

			pMP->ComputeDistinctiveDescriptors();
			pMP->UpdateNormalAndDepth();

			m_Map->AddMapPoint(pMP);
			m_listRecentAddedMapPoints.push_back(pMP);

			nnew++;
		}
	}
}


cv::Mat LocalMapping::ComputeF12(KeyFrame* &pKF1, KeyFrame* &pKF2)
{
	cv::Mat R1w = pKF1->GetRotation();
	cv::Mat t1w = pKF1->GetTranslation();
	cv::Mat R2w = pKF2->GetRotation();
	cv::Mat t2w = pKF2->GetTranslation();

	cv::Mat R12 = R1w * R2w.t();
	cv::Mat t12 = -R1w * R2w.t() * t2w + t1w;

	cv::Mat t12x = SkewSymmetricMatrix(t12);

	const cv::Mat &K1 = pKF1->m_K;
	const cv::Mat &K2 = pKF2->m_K;

	return K1.t().inv() * t12x * R12 * K2.inv();
}


cv::Mat LocalMapping::SkewSymmetricMatrix(const cv::Mat &v)
{
	return (cv::Mat_<float>(3,3) <<             0, -v.at<float>(2), v.at<float>(1),
	v.at<float>(2),               0,-v.at<float>(0),
	-v.at<float>(1),  v.at<float>(0),              0);
}


int LocalMapping::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F12,
									vector<pair<size_t, size_t> > &vMatchedPairs)
{
	//Compute epipole in second image
	cv::Mat Cw = pKF1->GetCameraCenter();
	cv::Mat R2w = pKF2->GetRotation();
	cv::Mat t2w = pKF2->GetTranslation();
	cv::Mat C2 = R2w * Cw + t2w;

	const float invz = 1.0f / C2.at<float>(2);
	const float ex = pKF2->m_fx * C2.at<float>(0) * invz + pKF2->m_cx;
	const float ey = pKF2->m_fy * C2.at<float>(1) * invz + pKF2->m_cy;

	vector<int> vMatches12(pKF1->m_keyPointsNumber, -1);

	int nmatches = 0;

	Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce");
	std::vector<DMatch> matches;

	matcher->match(pKF1->m_Descriptors, pKF2->m_Descriptors, matches);

	vector<Point2f>  KF1Point, KF2Point;
	for(size_t i=0; i<matches.size(); i++)
	{
		KF1Point.push_back(pKF1->m_vectorKeyPointsUn[matches[i].queryIdx].pt);
		KF2Point.push_back(pKF2->m_vectorKeyPointsUn[matches[i].trainIdx].pt);
	}

	vector<char> inLiners;
	Mat homography = findHomography(KF1Point, KF2Point,inLiners, RANSAC, 30.0);

	if(homography.empty())
	{
		return 0;
	}
	else
	{
		for(size_t i=0; i<KF1Point.size(); i++)
		{
			if(inLiners[i] !=0)
			{
				MapPoint* pMP1 = pKF1->GetMapPoint(matches[i].queryIdx);

                if(pMP1)
                    continue;

                const bool bStereo1 = pKF1->m_vectorURight[matches[i].queryIdx]>=0;
				if(!bStereo1)
				{
					continue;
				}

				const cv::KeyPoint &kp1 = pKF1->m_vectorKeyPointsUn[matches[i].queryIdx];

				MapPoint* pMP2 = pKF2->GetMapPoint(matches[i].trainIdx);

				if(pMP2)
                	continue;

				const bool bStereo2 = pKF2->m_vectorURight[matches[i].trainIdx]>=0;

				if(!bStereo2)
				{
					continue;
				}

				const cv::KeyPoint &kp2 = pKF2->m_vectorKeyPointsUn[matches[i].trainIdx];
				if(!bStereo1 && !bStereo2)
				{
					const float distex = ex - kp2.pt.x;
					const float distey = ey - kp2.pt.y;
					if(distex*distex + distey*distey < 100 * 1.2f)
					{
						continue;
					}
                }

				if(!CheckDistEpipolarLine(kp1, kp2, F12, pKF2))
				{
					continue;
				}

				vMatches12[matches[i].queryIdx] = matches[i].trainIdx;

				nmatches++;
			}
		}
	}

	vMatchedPairs.clear();
    vMatchedPairs.reserve(nmatches);

    for(size_t i=0, iend=vMatches12.size(); i<iend; i++)
    {
        if(vMatches12[i]<0)
            continue;
        vMatchedPairs.push_back(make_pair(i,vMatches12[i]));
    }

	return nmatches;
}

bool LocalMapping::CheckDistEpipolarLine(const cv::KeyPoint &kp1,const cv::KeyPoint &kp2,const cv::Mat &F12,const KeyFrame* pKF2)
{
	// Epipolar line in second image l = x1'F12 = [a b c]
	const float a = kp1.pt.x * F12.at<float>(0,0) + kp1.pt.y * F12.at<float>(1,0) + F12.at<float>(2,0);
	const float b = kp1.pt.x * F12.at<float>(0,1) + kp1.pt.y * F12.at<float>(1,1) + F12.at<float>(2,1);
	const float c = kp1.pt.x * F12.at<float>(0,2) + kp1.pt.y * F12.at<float>(1,2) + F12.at<float>(2,2);

	const float num = a * kp2.pt.x + b * kp2.pt.y + c;

	const float den = a * a + b * b;

	if(den == 0)
	{
		return false;
	}

	const float dsqr = num * num / den;

	return dsqr < 3.84 * (1.0f / 0.69f);
}


void LocalMapping::MapPointCulling()
{
	// Check Recent Added MapPoints
	list<MapPoint*>::iterator it = m_listRecentAddedMapPoints.begin();
	unsigned long int nCurrentKFid = m_CurrentKeyFrame->m_KeyFrameId;

	while(it != m_listRecentAddedMapPoints.end())
	{
		MapPoint* pMP = *it;
		if(pMP->isBadMapPoint())
		{
			it = m_listRecentAddedMapPoints.erase(it);
		}
		else if(pMP->GetFoundRatio() < 0.25f)
		{
			pMP->SetBadMapPointFlag();
			it = m_listRecentAddedMapPoints.erase(it);
		}
		else if(((int)nCurrentKFid-(int)pMP->m_FirstKeyFrameId) >= 2 && pMP->Observations() <= 3)
		{
			pMP->SetBadMapPointFlag();
            it = m_listRecentAddedMapPoints.erase(it);
		}
		else if(((int)nCurrentKFid-(int)pMP->m_FirstKeyFrameId) >= 3)
		{
			it = m_listRecentAddedMapPoints.erase(it);
		}
		else
		{
			it++;
		}
	}
}


void LocalMapping::SearchInNeighbors()
{
	// Retrieve neighbor keyframes
	int nn = 10;
	const vector<KeyFrame*> vpNeighKFs = m_CurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);

	vector<KeyFrame*> vpTargetKFs;
	for(vector<KeyFrame*>::const_iterator vit=vpNeighKFs.begin(), vend=vpNeighKFs.end(); vit!=vend; vit++)
	{
		KeyFrame* pKFi = *vit;
		if(pKFi->isBadKeyFrame() || pKFi->m_FuseTargetForKF == m_CurrentKeyFrame->m_KeyFrameId)
		{
			continue;
		}

		vpTargetKFs.push_back(pKFi);
		pKFi->m_FuseTargetForKF = m_CurrentKeyFrame->m_KeyFrameId;

		// Extend to some second neighbors
		const vector<KeyFrame*> vpSecondNeighKFs = pKFi->GetBestCovisibilityKeyFrames(5);
		for(vector<KeyFrame*>::const_iterator vit2=vpSecondNeighKFs.begin(), vend2=vpSecondNeighKFs.end(); vit2!=vend2; vit2++)
		{
			KeyFrame* pKFi2 = *vit2;
			if(pKFi2->isBadKeyFrame() || pKFi2->m_FuseTargetForKF==m_CurrentKeyFrame->m_KeyFrameId || pKFi2->m_KeyFrameId==m_CurrentKeyFrame->m_KeyFrameId)
			{
				continue;
			}

			vpTargetKFs.push_back(pKFi2);
		}
		
	}

	// Search matches by projection from current KF in target KFs
	vector<MapPoint*> vpMapPointMatches = m_CurrentKeyFrame->GetMapPointMatches();
	for(vector<KeyFrame*>::iterator vit=vpTargetKFs.begin(), vend=vpTargetKFs.end(); vit!=vend; vit++)
	{
		KeyFrame* pKFi = *vit;
		Fuse(pKFi,vpMapPointMatches);
	}

	// Search matches by projection from target KFs in current KF
	vector<MapPoint*> vpFuseCandidates;
	vpFuseCandidates.reserve(vpTargetKFs.size() * vpMapPointMatches.size());

	for(vector<KeyFrame*>::iterator vitKF=vpTargetKFs.begin(), vendKF=vpTargetKFs.end(); vitKF!=vendKF; vitKF++)
	{
		KeyFrame* pKFi = *vitKF;
		vector<MapPoint*> vpMapPointsKFi = pKFi->GetMapPointMatches();
		for(vector<MapPoint*>::iterator vitMP=vpMapPointsKFi.begin(), vendMP=vpMapPointsKFi.end(); vitMP!=vendMP; vitMP++)
		{
			MapPoint* pMP = *vitMP;
			if(!pMP)
			{
				continue;
			}

			if(pMP->isBadMapPoint() || pMP->m_FuseCandidateForKeyFrameId == m_CurrentKeyFrame->m_KeyFrameId)
			{
				continue;
			}

			pMP->m_FuseCandidateForKeyFrameId = m_CurrentKeyFrame->m_KeyFrameId;
			vpFuseCandidates.push_back(pMP);
		}
	}

	Fuse(m_CurrentKeyFrame,vpFuseCandidates);

	// Update points
	vpMapPointMatches = m_CurrentKeyFrame->GetMapPointMatches();
	for(size_t i=0, iend=vpMapPointMatches.size(); i<iend; i++)
	{
		 MapPoint* pMP = vpMapPointMatches[i];
		 if(pMP)
		 {
			if(!pMP->isBadMapPoint())
			{
				pMP->ComputeDistinctiveDescriptors();
				pMP->UpdateNormalAndDepth();
			}
		 }
	}

	// Update connections in covisibility graph
	m_CurrentKeyFrame->UpdateConnections();
}



int LocalMapping::Fuse(KeyFrame *pKF, const vector<MapPoint *> &vpMapPoints, const float th)
{
	cv::Mat Rcw = pKF->GetRotation();
	cv::Mat tcw = pKF->GetTranslation();

	float fx = pKF->m_fx;
	float fy = pKF->m_fy;
	float cx = pKF->m_cx;
	float cy = pKF->m_cy;
	float bf = pKF->m_baselineFx;

	cv::Mat Ow = pKF->GetCameraCenter();

	int nFused=0;
	int nMPs = vpMapPoints.size();
	for(int i=0; i<nMPs; i++)
	{
		MapPoint* pMP = vpMapPoints[i];
		if(!pMP)
		{
			continue;
		}

		if(pMP->isBadMapPoint() || pMP->IsInKeyFrame(pKF))
		{
			continue;
		}

		cv::Mat p3Dw = pMP->GetWorldPos();
		cv::Mat p3Dc = Rcw*p3Dw + tcw;

		// Depth must be positive
		if(p3Dc.at<float>(2) < 0.0f)
		{
			continue;
		}

		float invz = 1 / p3Dc.at<float>(2);
		float x = p3Dc.at<float>(0) * invz;
		float y = p3Dc.at<float>(1) * invz;


		float u = fx * x + cx;
		float v = fy * y + cy;

		// Point must be inside the image
		if(!pKF->IsInImage(u,v))
		{
			continue;
		}

		float ur = u - bf * invz;
		cv::Mat PO = p3Dw - Ow;
		float dist3D = cv::norm(PO);
		
		// Viewing angle must be less than 60 deg
		cv::Mat Pn = pMP->GetNormal();
		if(PO.dot(Pn) < 0.5 * dist3D)
		{
			continue;
		}

		vector<size_t> vIndices = pKF->GetFeaturesInArea(u, v, th);

		if(vIndices.empty())
		{
			continue;
		}

		// Match to the most similar keypoint in the radius
		cv::Mat dMP = pMP->GetDescriptor();
		int bestDist = INT32_MAX;
		int bestIdx = -1;
		for(vector<size_t>::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++)
		{
			size_t idx = *vit;
			const cv::KeyPoint &kp = pKF->m_vectorKeyPointsUn[idx];

			if(pKF->m_vectorURight[idx] >= 0)
			{
				// Check reprojection error in stereo
				float kpx = kp.pt.x;
				float kpy = kp.pt.y;
				float kpr = pKF->m_vectorURight[idx];
				float ex = u - kpx;
				float ey = v - kpy;
				float er = ur - kpr;
				float e2 = ex*ex + ey*ey + er*er;

				if(e2 * 0.69f > 7.815)
				{
					continue;
				}
			}
			else
			{
				float kpx = kp.pt.x;
				float kpy = kp.pt.y;
				float ex = u-kpx;
				float ey = v-kpy;
				float e2 = ex*ex + ey*ey;

				if(e2 * 0.69f > 5.991)
				{
					continue;
				}
			}

			const cv::Mat &dKF = pKF->m_Descriptors.row(idx);
			int dist = cv::norm(dMP,dKF);

			if(dist < bestDist)
			{
				bestDist = dist;
				bestIdx = idx;
			}
		}

		// If there is already a MapPoint replace otherwise add new measurement
		if(bestDist <= TH_LOW)
		{
			MapPoint* pMPinKF = pKF->GetMapPoint(bestIdx);
			if(pMPinKF)
			{
				if(!pMPinKF->isBadMapPoint())
				{
					if(pMPinKF->Observations() > pMP->Observations())
					{
						pMP->Replace(pMPinKF);
					}
					else
					{
						pMPinKF->Replace(pMP);
					}
				}
			}
			else
			{
				pMP->AddObservation(pKF, bestIdx);
				pKF->AddMapPoint(pMP, bestIdx);
			}

			nFused++;
		}
	}

	return nFused;
}


void LocalMapping::KeyFrameCulling()
{
	// Check redundant keyframes (only local keyframes)
    // A keyframe is considered redundant if the 90% of the MapPoints it sees, are seen
    // in at least other 3 keyframes (in the same or finer scale)
    // We only consider close stereo points
    vector<KeyFrame*> vpLocalKeyFrames = m_CurrentKeyFrame->GetVectorCovisibleKeyFrames();

	for(vector<KeyFrame*>::iterator vit=vpLocalKeyFrames.begin(), vend=vpLocalKeyFrames.end(); vit!=vend; vit++)
	{
		KeyFrame* pKF = *vit;
		if(pKF->m_KeyFrameId == 0)
		{
			continue;
		}

		const vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();

		const int thObs = 3;
		int nRedundantObservations = 0;
		int nMPs=0;
		for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
		{
			MapPoint* pMP = vpMapPoints[i];
			if(pMP)
			{
				if(!pMP->isBadMapPoint())
				{
					if(pKF->m_vectorDepth[i] > pKF->m_ThDepth || pKF->m_vectorDepth[i] < 0)
					{
						continue;
					}

					nMPs++;
					if(pMP->Observations() > thObs)
					{
						const map<KeyFrame*, size_t> observations = pMP->GetObservations();
						int nObs = 0;
						for(map<KeyFrame*, size_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
						{
							KeyFrame* pKFi = mit->first;
							if(pKFi == pKF)
							{
								continue;
							}

							nObs++;
							if(nObs >= thObs)
							{
								break;
							}
						}

						if(nObs >= thObs)
						{
							nRedundantObservations++;
						}
					}
				}
			}
		}

		if(nRedundantObservations > 0.9 * nMPs)
		{
			pKF->SetBadFlag();
		}
	}
}


void LocalMapping::ResetIfRequested()
{
	unique_lock<mutex> lock(m_MutexReset);
	if(m_ResetRequested)
	{
		m_listNewKeyFrames.clear();
		m_listRecentAddedMapPoints.clear();
		m_ResetRequested=false;
	}
}


bool LocalMapping::CheckFinish()
{
	unique_lock<mutex> lock(m_MutexFinish);
	return m_FinishRequested;
}


void LocalMapping::SetFinish()
{
	{
		unique_lock<mutex> lock(m_MutexFinish);
		m_Finished = true;
	}

	{
		unique_lock<mutex> lock2(m_MutexStop);
		m_Stopped = true;
	}
}


int LocalMapping::KeyframesInQueue()
{
	unique_lock<std::mutex> lock(m_MutexNewKFs);
	return m_listNewKeyFrames.size();
}



void LocalMapping::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap)
{
    // Local KeyFrames: First Breath Search from Current Keyframe
    list<KeyFrame*> lLocalKeyFrames;

    lLocalKeyFrames.push_back(pKF);
    pKF->m_BALocalForKF = pKF->m_KeyFrameId;

    const vector<KeyFrame*> vNeighKFs = pKF->GetVectorCovisibleKeyFrames();
    for(int i=0, iend=vNeighKFs.size(); i<iend; i++)
    {
        KeyFrame* pKFi = vNeighKFs[i];
        pKFi->m_BALocalForKF = pKF->m_KeyFrameId;
        if(!pKFi->isBadKeyFrame())
            lLocalKeyFrames.push_back(pKFi);
    }

    // Local MapPoints seen in Local KeyFrames
    list<MapPoint*> lLocalMapPoints;
    for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin() , lend=lLocalKeyFrames.end(); lit!=lend; lit++)
    {
        vector<MapPoint*> vpMPs = (*lit)->GetMapPointMatches();
        for(vector<MapPoint*>::iterator vit=vpMPs.begin(), vend=vpMPs.end(); vit!=vend; vit++)
        {
            MapPoint* pMP = *vit;
            if(pMP)
                if(!pMP->isBadMapPoint())
                    if(pMP->m_BALocalForKeyFrameId != pKF->m_KeyFrameId)
                    {
                        lLocalMapPoints.push_back(pMP);
                        pMP->m_BALocalForKeyFrameId = pKF->m_KeyFrameId;
                    }
        }
    }

    // Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframes
    list<KeyFrame*> lFixedCameras;
    for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
    {
        map<KeyFrame*,size_t> observations = (*lit)->GetObservations();
        for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
        {
            KeyFrame* pKFi = mit->first;

            if(pKFi->m_BALocalForKF!=pKF->m_KeyFrameId && pKFi->m_BAFixedForKF!=pKF->m_KeyFrameId)
            {                
                pKFi->m_BAFixedForKF=pKF->m_KeyFrameId;
                if(!pKFi->isBadKeyFrame())
                    lFixedCameras.push_back(pKFi);
            }
        }
    }

    // Setup optimizer
    g2o::SparseOptimizer optimizer;
    g2o::BlockSolver_6_3::LinearSolverType * linearSolver;

    linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();

    g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);

    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    optimizer.setAlgorithm(solver);

    if(pbStopFlag)
        optimizer.setForceStopFlag(pbStopFlag);

    unsigned long maxKFid = 0;

    // Set Local KeyFrame vertices
    for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
    {
        KeyFrame* pKFi = *lit;
        g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
        vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
        vSE3->setId(pKFi->m_KeyFrameId);
        vSE3->setFixed(pKFi->m_KeyFrameId==0);
        optimizer.addVertex(vSE3);
        if(pKFi->m_KeyFrameId > maxKFid)
            maxKFid=pKFi->m_KeyFrameId;
    }

    // Set Fixed KeyFrame vertices
    for(list<KeyFrame*>::iterator lit=lFixedCameras.begin(), lend=lFixedCameras.end(); lit!=lend; lit++)
    {
        KeyFrame* pKFi = *lit;
        g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
        vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
        vSE3->setId(pKFi->m_KeyFrameId);
        vSE3->setFixed(true);
        optimizer.addVertex(vSE3);
        if(pKFi->m_KeyFrameId > maxKFid)
            maxKFid=pKFi->m_KeyFrameId;
    }

    // Set MapPoint vertices
    const int nExpectedSize = (lLocalKeyFrames.size()+lFixedCameras.size())*lLocalMapPoints.size();

    vector<g2o::EdgeSE3ProjectXYZ*> vpEdgesMono;
    vpEdgesMono.reserve(nExpectedSize);

    vector<KeyFrame*> vpEdgeKFMono;
    vpEdgeKFMono.reserve(nExpectedSize);

    vector<MapPoint*> vpMapPointEdgeMono;
    vpMapPointEdgeMono.reserve(nExpectedSize);

    vector<g2o::EdgeStereoSE3ProjectXYZ*> vpEdgesStereo;
    vpEdgesStereo.reserve(nExpectedSize);

    vector<KeyFrame*> vpEdgeKFStereo;
    vpEdgeKFStereo.reserve(nExpectedSize);

    vector<MapPoint*> vpMapPointEdgeStereo;
    vpMapPointEdgeStereo.reserve(nExpectedSize);

    const float thHuberMono = sqrt(5.991);
    const float thHuberStereo = sqrt(7.815);

    for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
    {
        MapPoint* pMP = *lit;
        g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
        vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
        int id = pMP->m_MapPointId + maxKFid + 1;
        vPoint->setId(id);
        vPoint->setMarginalized(true);
        optimizer.addVertex(vPoint);

        const map<KeyFrame*,size_t> observations = pMP->GetObservations();

        //Set edges
        for(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
        {
            KeyFrame* pKFi = mit->first;

            if(!pKFi->isBadKeyFrame())
            {                
                const cv::KeyPoint &kpUn = pKFi->m_vectorKeyPointsUn[mit->second];

                // Monocular observation
                if(pKFi->m_vectorURight[mit->second]<0)
                {
                    Eigen::Matrix<double,2,1> obs;
                    obs << kpUn.pt.x, kpUn.pt.y;

                    g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();

                    e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
                    e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->m_KeyFrameId)));
                    e->setMeasurement(obs);
                    const float &invSigma2 = 0.69f;
                    e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);

                    g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                    e->setRobustKernel(rk);
                    rk->setDelta(thHuberMono);

                    e->fx = pKFi->m_fx;
                    e->fy = pKFi->m_fy;
                    e->cx = pKFi->m_cx;
                    e->cy = pKFi->m_cy;

                    optimizer.addEdge(e);
                    vpEdgesMono.push_back(e);
                    vpEdgeKFMono.push_back(pKFi);
                    vpMapPointEdgeMono.push_back(pMP);
                }
                else // Stereo observation
                {
                    Eigen::Matrix<double,3,1> obs;
                    const float kp_ur = pKFi->m_vectorURight[mit->second];
                    obs << kpUn.pt.x, kpUn.pt.y, kp_ur;

                    g2o::EdgeStereoSE3ProjectXYZ* e = new g2o::EdgeStereoSE3ProjectXYZ();

                    e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
                    e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->m_KeyFrameId)));
                    e->setMeasurement(obs);
                    const float &invSigma2 = 0.69f;
                    Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;
                    e->setInformation(Info);

                    g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                    e->setRobustKernel(rk);
                    rk->setDelta(thHuberStereo);

                    e->fx = pKFi->m_fx;
                    e->fy = pKFi->m_fy;
                    e->cx = pKFi->m_cx;
                    e->cy = pKFi->m_cy;
                    e->bf = pKFi->m_baselineFx;

                    optimizer.addEdge(e);
                    vpEdgesStereo.push_back(e);
                    vpEdgeKFStereo.push_back(pKFi);
                    vpMapPointEdgeStereo.push_back(pMP);
                }
            }
        }
    }

    if(pbStopFlag)
        if(*pbStopFlag)
            return;

    optimizer.initializeOptimization();
    optimizer.optimize(5);

    bool bDoMore= true;

    if(pbStopFlag)
        if(*pbStopFlag)
            bDoMore = false;

    if(bDoMore)
    {

    // Check inlier observations
    for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
    {
        g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
        MapPoint* pMP = vpMapPointEdgeMono[i];

        if(pMP->isBadMapPoint())
            continue;

        if(e->chi2()>5.991 || !e->isDepthPositive())
        {
            e->setLevel(1);
        }

        e->setRobustKernel(0);
    }

    for(size_t i=0, iend=vpEdgesStereo.size(); i<iend;i++)
    {
        g2o::EdgeStereoSE3ProjectXYZ* e = vpEdgesStereo[i];
        MapPoint* pMP = vpMapPointEdgeStereo[i];

        if(pMP->isBadMapPoint())
            continue;

        if(e->chi2()>7.815 || !e->isDepthPositive())
        {
            e->setLevel(1);
        }

        e->setRobustKernel(0);
    }

    // Optimize again without the outliers

    optimizer.initializeOptimization(0);
    optimizer.optimize(10);

    }

    vector<pair<KeyFrame*,MapPoint*> > vToErase;
    vToErase.reserve(vpEdgesMono.size()+vpEdgesStereo.size());

    // Check inlier observations       
    for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
    {
        g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
        MapPoint* pMP = vpMapPointEdgeMono[i];

        if(pMP->isBadMapPoint())
            continue;

        if(e->chi2()>5.991 || !e->isDepthPositive())
        {
            KeyFrame* pKFi = vpEdgeKFMono[i];
            vToErase.push_back(make_pair(pKFi,pMP));
        }
    }

    for(size_t i=0, iend=vpEdgesStereo.size(); i<iend;i++)
    {
        g2o::EdgeStereoSE3ProjectXYZ* e = vpEdgesStereo[i];
        MapPoint* pMP = vpMapPointEdgeStereo[i];

        if(pMP->isBadMapPoint())
            continue;

        if(e->chi2()>7.815 || !e->isDepthPositive())
        {
            KeyFrame* pKFi = vpEdgeKFStereo[i];
            vToErase.push_back(make_pair(pKFi,pMP));
        }
    }

    // Get Map Mutex
    unique_lock<mutex> lock(pMap->m_MutexMapUpdate);

    if(!vToErase.empty())
    {
        for(size_t i=0;i<vToErase.size();i++)
        {
            KeyFrame* pKFi = vToErase[i].first;
            MapPoint* pMPi = vToErase[i].second;
            pKFi->EraseMapPointMatch(pMPi);
            pMPi->EraseObservation(pKFi);
        }
    }

    // Recover optimized data

    //Keyframes
    for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
    {
        KeyFrame* pKF = *lit;
        g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->m_KeyFrameId));
        g2o::SE3Quat SE3quat = vSE3->estimate();
        pKF->SetPose(Converter::toCvMat(SE3quat));
    }

    //Points
    for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
    {
        MapPoint* pMP = *lit;
        g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->m_MapPointId + maxKFid + 1));
        pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
        pMP->UpdateNormalAndDepth();
    }
}

